package com.soonoh.simplecube.cube;

/**
 * A arc ball rotationer.
 * @author soonoh
 *
 */
public class ArcBall {
	//assuming IEEE-754(float), which i believe has max precision of 7 bits
	private static final float Epsilon = (float) 1.0e-5;
	
	private Vector3f clickVector = new Vector3f();
	private Vector3f dragVector = new Vector3f();
	private float adjustWidth;
	private float adjustHeight;
	
	public ArcBall() {
		this(0.0f, 0.0f);
	}

	public ArcBall(float width, float height) {
	    //Clear initial values
	    clickVector.X     =
	    clickVector.Y     =
	    clickVector.Z     =

	    dragVector.X     =
	    dragVector.Y     =
	    dragVector.Z     = 0.0f;

	    //Set initial bounds
	    setBounds(width, height);
	}
	
	public void setBounds(float newWidth, float newHeight) {
        assert((newWidth > 1.0f) && (newHeight > 1.0f));
        //Set adjustment factor for width/height
        adjustWidth  = 1.0f / ((newWidth  - 1.0f) * 0.5f);
        adjustHeight = 1.0f / ((newHeight - 1.0f) * 0.5f);
	}
	public void click(final Point2f NewPt)
	{
	    //Map the point to the sphere
	    mapToSphere(NewPt, clickVector);
	}
	public void drag(final Point2f NewPt, Quat4f NewRot)
	{
	    //Map the point to the sphere
	    mapToSphere(NewPt, dragVector);

	    //Return the quaternion equivalent to the rotation
	    if (NewRot != null)
	    {
	        Vector3f  Perp;

	        //Compute the vector perpendicular to the begin and end vectors
	        Perp = MathUtil.Vector3fCross(clickVector, dragVector);

	        //Compute the length of the perpendicular vector
	        if (MathUtil.Vector3fLength(Perp) > Epsilon)    //if its non-zero
	        {
	            //We're ok, so return the perpendicular vector as the transform after all
	            NewRot.X = Perp.X;
	            NewRot.Y = Perp.Y;
	            NewRot.Z = Perp.Z;
	            //In the quaternion values, w is cosine (theta / 2), where theta is rotation angle
	            NewRot.W= MathUtil.Vector3fDot(clickVector, dragVector);
	        }
	        else                                    //if its zero
	        {
	            //The begin and end vectors coincide, so return an identity transform
	            NewRot.X =
	            NewRot.Y =
	            NewRot.Z =
	            NewRot.W = 0.0f;
	        }
	    }
	}
	protected void mapToSphere(final Point2f NewPt, Vector3f NewVec) {
	    Point2f TempPt = new Point2f();
	    float length;

	    //Copy paramter into temp point
	    TempPt.x = NewPt.x;
	    TempPt.y = NewPt.y;

	    //Adjust point coords and scale down to range of [-1 ... 1]
	    TempPt.x =        (TempPt.x* adjustWidth)  - 1.0f;
	    TempPt.y  = 1.0f - (TempPt.y * adjustHeight);

	    //Compute the square of the length of the vector to the point from the center
	    length      = (TempPt.x* TempPt.x) + (TempPt.y * TempPt.y);

	    //If the point is mapped outside of the sphere... (length > radius squared)
	    if (length > 1.0f)
	    {
	        float norm;

	        //Compute a normalizing factor (radius / sqrt(length))
	        norm    = 1.0f / MathUtil.sqrt(length);

	        //Return the "normalized" vector, a point on the sphere
	        NewVec.X = TempPt.x* norm;
	        NewVec.Y = TempPt.y * norm;
	        NewVec.Z = 0.0f;
	    }
	    else    //Else it's on the inside
	    {
	        //Return a vector to a point mapped inside the sphere sqrt(radius squared - length)
	        NewVec.X = TempPt.x;
	        NewVec.Y = TempPt.y;
	        NewVec.Z = MathUtil.sqrt(1.0f - length);
	    }
	}
	
    /**
     * Sets the rotational component (upper 3x3) of this matrix to the matrix
     * values in the T precision Matrix3d argument; the other elements of
     * this matrix are unchanged; a singular value decomposition is performed
     * on this object's upper 3x3 matrix to factor out the scale, then this
     * object's upper 3x3 matrix components are replaced by the passed rotation
     * components, and then the scale is reapplied to the rotational
     * components.
     * @param m1 T precision 3x3 matrix
     */
	public static void Matrix4fSetRotationFromMatrix3f(Matrix4f NewObj, final Matrix3f m1)
    {
        float scale;

        scale = Matrix4fSVD(NewObj, null, null);

        Matrix4fSetRotationScaleFromMatrix3f(NewObj, m1);
        Matrix4fMulRotationScale(NewObj, scale);
    }

    private static void Matrix4fMulRotationScale(Matrix4f NewObj, float scale)
    {
        NewObj.m[Matrix4f.XX] *= scale; NewObj.m[Matrix4f.YX] *= scale; NewObj.m[Matrix4f.ZX] *= scale;
        NewObj.m[Matrix4f.XY] *= scale; NewObj.m[Matrix4f.YY] *= scale; NewObj.m[Matrix4f.ZY] *= scale;
        NewObj.m[Matrix4f.XZ] *= scale; NewObj.m[Matrix4f.YZ] *= scale; NewObj.m[Matrix4f.ZZ] *= scale;
    }

	private static void Matrix4fSetRotationScaleFromMatrix3f(Matrix4f NewObj, final Matrix3f m1)
    {
        NewObj.m[Matrix4f.XX] = m1.XX(); NewObj.m[Matrix4f.YX] = m1.YX(); NewObj.m[Matrix4f.ZX] = m1.ZX();
        NewObj.m[Matrix4f.XY] = m1.XY(); NewObj.m[Matrix4f.YY] = m1.YY(); NewObj.m[Matrix4f.ZY] = m1.ZY();
        NewObj.m[Matrix4f.XZ] = m1.XZ(); NewObj.m[Matrix4f.YZ] = m1.YZ(); NewObj.m[Matrix4f.ZZ] = m1.ZZ();
    }

	/**
      * Performs SVD on this matrix and gets scale and rotation.
      * Rotation is placed into rot3, and rot4.
      * @param rot3 the rotation factor(Matrix3d). if null, ignored
      * @param rot4 the rotation factor(Matrix4) only upper 3x3 elements are changed. if null, ignored
      * @return scale factor
      */
	private static float Matrix4fSVD(final Matrix4f NewObj, Matrix3f rot3, Matrix4f rot4) {
        float s, n;

        // this is a simple svd.
        // Not complete but fast and reasonable.
        // See comment in Matrix3d.

        s = MathUtil.sqrt(
                ( (NewObj.XX() * NewObj.XX()) + (NewObj.XY() * NewObj.XY()) + (NewObj.XZ() * NewObj.XZ()) +
                  (NewObj.YX() * NewObj.YX()) + (NewObj.YY() * NewObj.YY()) + (NewObj.YZ() * NewObj.YZ()) +
                  (NewObj.ZX() * NewObj.ZX()) + (NewObj.ZY() * NewObj.ZY()) + (NewObj.ZZ() * NewObj.ZZ()) ) / 3.0f );

        if (rot3 != null)
        {
            //this->getRotationScale(rot3);
            rot3.m[Matrix3f.XX] = NewObj.XX(); rot3.m[Matrix3f.XY] = NewObj.XY(); rot3.m[Matrix3f.XZ] = NewObj.XZ();
            rot3.m[Matrix3f.YX] = NewObj.YX(); rot3.m[Matrix3f.YY] = NewObj.YY(); rot3.m[Matrix3f.YZ] = NewObj.YZ();
            rot3.m[Matrix3f.ZX] = NewObj.ZX(); rot3.m[Matrix3f.ZY] = NewObj.ZY(); rot3.m[Matrix3f.ZZ] = NewObj.ZZ();

            // zero-div may occur.

            n = 1.0f / MathUtil.sqrt( (NewObj.XX() * NewObj.XX()) +
                                      (NewObj.XY() * NewObj.XY()) +
                                      (NewObj.XZ() * NewObj.XZ()) );
            rot3.m[Matrix3f.XX] *= n;
            rot3.m[Matrix3f.XY] *= n;
            rot3.m[Matrix3f.XZ] *= n;

            n = 1.0f / MathUtil.sqrt( (NewObj.YX() * NewObj.YX()) +
                                      (NewObj.YY() * NewObj.YY()) +
                                      (NewObj.YZ() * NewObj.YZ()) );
            rot3.m[Matrix3f.YX] *= n;
            rot3.m[Matrix3f.YY] *= n;
            rot3.m[Matrix3f.YZ] *= n;

            n = 1.0f / MathUtil.sqrt( (NewObj.ZX() * NewObj.ZX()) +
                                      (NewObj.ZY() * NewObj.ZY()) +
                                      (NewObj.ZZ() * NewObj.ZZ()) );
            rot3.m[Matrix3f.ZX] *= n;
            rot3.m[Matrix3f.ZY] *= n;
            rot3.m[Matrix3f.ZZ] *= n;
        }

        if (rot4 != null)
        {
            if (rot4 != NewObj)
            {
                Matrix4fSetRotationScaleFromMatrix4f(rot4, NewObj);  // private method
            }

            // zero-div may occur.

            n = 1.0f / MathUtil.sqrt( (NewObj.XX() * NewObj.XX()) +
                                      (NewObj.XY() * NewObj.XY()) +
                                      (NewObj.XZ() * NewObj.XZ()) );
            rot4.m[Matrix4f.XX] *= n;
            rot4.m[Matrix4f.XY] *= n;
            rot4.m[Matrix4f.XZ] *= n;

            n = 1.0f / MathUtil.sqrt( (NewObj.YX() * NewObj.YX()) +
                                      (NewObj.YY() * NewObj.YY()) +
                                      (NewObj.YZ() * NewObj.YZ()) );
            rot4.m[Matrix4f.YX] *= n;
            rot4.m[Matrix4f.YY] *= n;
            rot4.m[Matrix4f.YZ] *= n;

            n = 1.0f / MathUtil.sqrt( (NewObj.ZX() * NewObj.ZX()) +
                                      (NewObj.ZY() * NewObj.ZY()) +
                                      (NewObj.ZZ() * NewObj.ZZ()) );
            rot4.m[Matrix4f.ZX] *= n;
            rot4.m[Matrix4f.ZY] *= n;
            rot4.m[Matrix4f.ZZ] *= n;
        }

        return s;
	}

	private static void Matrix4fSetRotationScaleFromMatrix4f(Matrix4f NewObj, final Matrix4f m1)
    {
        NewObj.m[Matrix4f.XX] = m1.XX(); NewObj.m[Matrix4f.YX] = m1.YX(); NewObj.m[Matrix4f.ZX] = m1.ZX();
        NewObj.m[Matrix4f.XY] = m1.XY(); NewObj.m[Matrix4f.YY] = m1.YY(); NewObj.m[Matrix4f.ZY] = m1.ZY();
        NewObj.m[Matrix4f.XZ] = m1.XZ(); NewObj.m[Matrix4f.YZ] = m1.YZ(); NewObj.m[Matrix4f.ZZ] = m1.ZZ();
    }

	public static void Matrix3fSetRotationFromQuat4f(Matrix3f NewObj, final Quat4f q1)
    {
        float n, s;
        float xs, ys, zs;
        float wx, wy, wz;
        float xx, xy, xz;
        float yy, yz, zz;

        n = (q1.X * q1.X) + (q1.Y * q1.Y) + (q1.Z * q1.Z) + (q1.W * q1.W);
        s = (n > 0.0f) ? (2.0f / n) : 0.0f;

        xs = q1.X * s;  ys = q1.Y * s;  zs = q1.Z * s;
        wx = q1.W * xs; wy = q1.W * ys; wz = q1.W * zs;
        xx = q1.X * xs; xy = q1.X * ys; xz = q1.X * zs;
        yy = q1.Y * ys; yz = q1.Y * zs; zz = q1.Z * zs;

        NewObj.m[Matrix3f.XX] = 1.0f - (yy + zz); NewObj.m[Matrix3f.YX] =         xy - wz;  NewObj.m[Matrix3f.ZX] =         xz + wy;
        NewObj.m[Matrix3f.XY] =         xy + wz;  NewObj.m[Matrix3f.YY] = 1.0f - (xx + zz); NewObj.m[Matrix3f.ZY] =         yz - wx;
        NewObj.m[Matrix3f.XZ] =         xz - wy;  NewObj.m[Matrix3f.YZ] =         yz + wx;  NewObj.m[Matrix3f.ZZ] = 1.0f - (xx + yy);
    }
	
	public static Matrix3f Matrix3fMulMatrix3f(Matrix3f NewObj, final Matrix3f m1)
    {
        Matrix3f Result = new Matrix3f();

        // alias-safe way.
        NewObj.multiply(m1);
        return NewObj;
    }

}

class MathUtil {
	public static float sqrt(float f) {
		return (float) Math.sqrt((double)f);
	}
	public static float Vector3fDot(Vector3f v1, Vector3f v2) {
        assert(v1 != null && v2 != null);

        return  (v1.X * v2.X) +
                (v1.Y * v2.Y) +
                (v1.Z * v2.Z);
	}
	public static Vector3f Vector3fCross(Vector3f v1, Vector3f v2) {
        Vector3f Result = new Vector3f();

        assert(v1 != null && v2 != null);

        // store on stack once for aliasing-safty
        // i.e. safe when a.cross(a, b)

        Result.X = (v1.Y * v2.Z) - (v1.Z * v2.Y);
        Result.Y = (v1.Z * v2.X) - (v1.X * v2.Z);
        Result.Z = (v1.X * v2.Y) - (v1.Y * v2.X);

        //copy result back
        return Result;
	}

	public static float Vector3fLength(Vector3f vec) {
		return sqrt(Vector3fLengthSquared(vec));
	}

    private static float Vector3fLengthSquared(final Vector3f vec)
    {
        return  (vec.X * vec.X) +
                (vec.Y * vec.Y) +
                (vec.Z * vec.Z);
    }
}
